%Author:Xiyin,li
%Date  :2021-07-12
%Copyright 2021 Xiyin,li.

%% Introfuction
%Calculate the force between permanent magnet
%By Dipole Model and Magnet Moment
%       |z
%       |
%       |_________y
%      /
%   x/
clc,clear
close all
%% STEP1:Set Parameters
Br = 1.465;
nu0 = 4*pi*10^(-7);
effector.radius = 5e-3;
effector.diameter = 2*effector.radius;
effector.height = 10e-3;
effector.volume = pi*(effector.radius^2)*effector.height;
effector.magDirection = [0;-1;0];
effector.postion.Cur = [0;0;0];
effector.attitude0 = [0;0;0];

driver.radius = 15e-3;
driver.diameter = 2*driver.radius;
driver.height = 30e-3;
driver.volume = pi*(driver.radius^2)*driver.height;
driver.magDirection = [0;0;1];
driver.position.Cur = [20e-3;100e-3;10e-3];
driver.attitude0 = [pi/2;0;0];

%% STEP2:Calculate Magnetic Moment[01] and Numercial diff.
effector.magMoment = effector.magDirection*(Br*effector.volume)/nu0;
driver.magMoment =   driver.magDirection*(Br*  driver.volume)/nu0;

%% STEP3:Calculate Magnetic induction
effector.magInduction.Ana.diffMatrix = diffMatrix(effector,driver.position.Cur,'ana');
%effector.magInduction.Dip.diffMatrix = diffMatrix(effector,driver.position.Cur,'dip');

%% STEP4.1:Calculate Force[02]
% F.Ana = [];
% F.Dip = [];
% for Distance  = linspace(50e-3,150e-3,361)
%     driver.position.Cur = [0;Distance;0];
%     driver.attitude = [0;0;0]+driver.attitude0;
%     driver.rotationMatrix = Rot3(driver.attitude);
%     effector.magInduction.Ana.diffMatrix = diffMatrix(effector,driver.position.Cur,'ana');
%     F.Ana = [F.Ana (effector.magInduction.Ana.diffMatrix)*driver.rotationMatrix'*driver.magMoment];
%     %F.DipDiff = (effector.magInduction.Dip.diffMatrix)*driver.rotationMatrix'*driver.magMoment
%     F.Dip = [F.Dip ForceDip(effector.magMoment,driver.magMoment,driver.rotationMatrix,driver.position.Cur)];
% end
%% STEP4.1:Calculate Force[02]
F = [];

for theta = linspace(0,2*pi,361)
    att = [theta;0;0];
    rot = Rot3(att);
    M1 = rot*effector.magDirection*(Br*effector.volume)/nu0;
    M2 = rot*[0;-1;0]*(Br*  driver.volume)/nu0;
    F  = [F [ForceDip(M1,M2,eye(3),driver.position.Cur);theta]];
end
F(5,:) = sqrt(F(1,:).^2+F(2,:).^2+F(3,:).^2);
%% STEP5:Calculate by intergral
% [nums.radius,nums.theta,nums.height] =  deal(10,10,10);
% step.radius = driver.radius/nums.theta;
% step.theta = 2*pi/nums.theta;
% step.height= driver.height/nums.theta;
% [ii,jj,kk] = deal(1,1,1);
% FX = zeros(nums.theta,nums.radius,nums.height);
% FY = zeros(nums.theta,nums.radius,nums.height);
% FZ = zeros(nums.theta,nums.radius,nums.height);
% for theta = linspace()




%% STEP6:Debug & Plot
figure(1)
hold on;grid on
% set(gca,'FontSize',60,'Fontname', 'Times New Roman');
% plot(1000*linspace(50e-3,150e-3,101),-F.Ana(2,:),'b-v','lineWidth',6);
% plot(1000*linspace(50e-3,150e-3,101),-F.Dip(2,:),'r-.o','lineWidth',6);
% xlabel('Distance (mm)');
% ylabel('-F(N)')
% legend('Analytical Model','Dipole Model')
% plotGif(linspace(50e-3,150e-3,361),-F.Ana(2,:),-F.Dip(2,:),'F')
plot(F(4,:),F(1,:),F(4,:),F(2,:),F(4,:),F(3,:),F(4,:),F(5,:));
legend('x','y','z','||')
%% Summary
%% Reference
%[01]
%[02]

%% Libary
function R = Rot3(attitude)
ax = attitude(1);
by = attitude(2);
cz = attitude(3);
Rz = [cos(cz) -sin(cz) 0;sin(cz) cos(cz) 0;0 0 1];
Ry = [cos(by) 0 sin(by);0 1 0;-sin(by) 0 cos(by)];
Rx = [1 0 0;0 cos(ax) -sin(ax);0 sin(ax) cos(ax)];
R = Rz*Ry*Rx;
end

function F = ForceDip(M1,M2,Rot,position)
nu0 = 4*pi*10^(-7);
D = norm(position);
r = position;
mul1 = (3*nu0)/(4*pi*D^4);
mul2 = M1*r'/D+r*M1'/D-((5*r*r')/(D^2)-eye(3))*(M1'*r/D);
mul3 = Rot*M2;
F = mul1*mul2*mul3;
end


function diff = diffMatrix(effector,position,method)
% 计算梯度矩阵，其中efferctor是磁源头，method是选用的方法
Br = 1.465;
dx = 0.01e-3;dy =0.01e-3;dz=0.01e-3;
driver.position.Cur = position;
driver.position.xPre  =  driver.position.Cur - [dx/2;0;0];
driver.position.xNext =  driver.position.Cur + [dx/2;0;0];
driver.position.yPre  =  driver.position.Cur - [0;dy/2;0];
driver.position.yNext =  driver.position.Cur + [0;dy/2;0];
driver.position.zPre  =  driver.position.Cur - [0;0;dz/2];
driver.position.zNext =  driver.position.Cur + [0;0;dz/2];
switch method
    case 'ana'
        effector.magInduction.Ana.xPre  = PMCir3DByRadius(Br,effector.diameter,effector.height,driver.position.xPre);
        effector.magInduction.Ana.xNext = PMCir3DByRadius(Br,effector.diameter,effector.height,driver.position.xNext);
        effector.magInduction.Ana.yPre  = PMCir3DByRadius(Br,effector.diameter,effector.height,driver.position.yPre);
        effector.magInduction.Ana.yNext = PMCir3DByRadius(Br,effector.diameter,effector.height,driver.position.yNext);
        effector.magInduction.Ana.zPre  = PMCir3DByRadius(Br,effector.diameter,effector.height,driver.position.zPre);
        effector.magInduction.Ana.zNext = PMCir3DByRadius(Br,effector.diameter,effector.height,driver.position.zNext);
        effector.magInduction.Ana.diffMatrix = [(effector.magInduction.Ana.xNext-effector.magInduction.Ana.xPre)';...
            (effector.magInduction.Ana.yNext-effector.magInduction.Ana.yPre)';...
            (effector.magInduction.Ana.zNext-effector.magInduction.Ana.zPre)'  ];
        diff = effector.magInduction.Ana.diffMatrix/dx;
    case 'dip'
        effector.magInduction.Dip.xPre  = PMDipCir(effector.radius,effector.height,driver.position.xPre ,'y');
        effector.magInduction.Dip.xNext = PMDipCir(effector.radius,effector.height,driver.position.xNext,'y');
        effector.magInduction.Dip.yPre  = PMDipCir(effector.radius,effector.height,driver.position.yPre ,'y');
        effector.magInduction.Dip.yNext = PMDipCir(effector.radius,effector.height,driver.position.yNext,'y');
        effector.magInduction.Dip.zPre  = PMDipCir(effector.radius,effector.height,driver.position.zPre ,'y');
        effector.magInduction.Dip.zNext = PMDipCir(effector.radius,effector.height,driver.position.zNext,'y');
        effector.magInduction.Dip.diffMatrix = [(effector.magInduction.Dip.xNext-effector.magInduction.Dip.xPre)';...
            (effector.magInduction.Dip.yNext-effector.magInduction.Dip.yPre)';...
            (effector.magInduction.Dip.zNext-effector.magInduction.Dip.zPre)'   ];
        effector.magInduction.Ana.Cur = PMCir3DByRadius(Br,effector.diameter,effector.height,driver.position.Cur);
        effector.magInduction.Dip.Cur = PMDipCir(effector.radius,  effector.height,driver.position.Cur,'y');
        diff = effector.magInduction.Dip.diffMatrix/dx;
end
end